#include "MeasurE452AlgBackS1S2S4S5HighS3Circle.h"

MeasurE452AlgBackS1S2S4S5HighS3Circle::MeasurE452AlgBackS1S2S4S5HighS3Circle(int i):MeasurE452AlgBase(i)
{

}

void MeasurE452AlgBackS1S2S4S5HighS3Circle::operator()(cv::Mat &srcImgBack,
                                                       cv::Mat &srcImgHigh,
                                                       int id,
                                                       std::map<string, float> &measRes,
                                                       int fixtureId)
{
    qDebug() << "start to E452 alg back s1 s2 s4 s5 high s3 circle";
    //背光图小半圆边缘提取  // SS12 back图    \paras452\back\ SS.xml
//    int GetLineContours(cv::Mat srcimg, const vector<int> paras, vector<cv::Point>& contours);

    std::vector<std::vector<cv::Point2f>> resptBackS1Vec,resptBackS2Vec,resptBackS4Vec,resptBackS5Vec,resptHighS3Vec;
    resptBackS1Vec.resize(5);
    resptBackS2Vec.resize(4);
    resptBackS4Vec.resize(2);
    resptBackS5Vec.resize(2);
    resptHighS3Vec.resize(1);
#pragma omp parallel sections
    {
        #pragma omp section
        {
            //S1
            int i=22;
            for(auto k=0;k<5;k++)
            {
                std::vector<int> tparas;
                for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
                {
                    tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(k,j));
                }
                //paras.push_back(tparas);
                std::vector<cv::Rect> rangVec;

                if(srcImgBack.empty()) {
                    Q_ASSERT(0);
                }

                TemplateMatch(srcImgBack,m_dataAllVec[i],rangVec);
                cv::Mat imgt=srcImgBack(rangVec[0]).clone();
                std::vector<cv::Point> contours;
                MeasurE452EdgeDetect obj;
                obj.GetLineContours(imgt,tparas,contours);
                ImageCoordinateTransformation(contours,rangVec);

                std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
                while (!MeasurE452AlgBase::flagVec[id])
                {
                    (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
                }
                for(auto &elem:contours)
                {
                    cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
                    CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                    resptBackS1Vec[k].push_back(ptsOut);
                }
            }

        }
        #pragma omp section
        {
            //S2
            int i=23;
            for(auto k=5;k<9;k++)
            {
                std::vector<int> tparas;
                for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
                {
                    tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(k,j));
                }
                //paras.push_back(tparas);
                std::vector<cv::Rect> rangVec;

                if(srcImgBack.empty()) {
                    Q_ASSERT(0);
                }

                TemplateMatch(srcImgBack,m_dataAllVec[i],rangVec);
                cv::Mat imgt=srcImgBack(rangVec[0]).clone();
                std::vector<cv::Point> contours;
                MeasurE452EdgeDetect obj;
                obj.GetLineContours(imgt,tparas,contours);
                ImageCoordinateTransformation(contours,rangVec);

                std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
                while (!MeasurE452AlgBase::flagVec[id])
                {
                    (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
                }
                for(auto &elem:contours)
                {
                    cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
                    CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                    resptBackS2Vec[k-5].push_back(ptsOut);
                }
            }
        }


        #pragma omp section
        {
            //S4
            int i=24;
            for(auto k=9;k<11;k++)
            {
                std::vector<int> tparas;
                for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
                {
                    tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(k,j));
                }
                //paras.push_back(tparas);
                std::vector<cv::Rect> rangVec;

                if(srcImgBack.empty()) {
                    Q_ASSERT(0);
                }

                TemplateMatch(srcImgBack,m_dataAllVec[i],rangVec);
                cv::Mat imgt=srcImgBack(rangVec[0]).clone();
                std::vector<cv::Point> contours;
                MeasurE452EdgeDetect obj;
                obj.GetLineContours(imgt,tparas,contours);
                ImageCoordinateTransformation(contours,rangVec);

                std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
                while (!MeasurE452AlgBase::flagVec[id])
                {
                    (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
                }
                for(auto &elem:contours)
                {
                    cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
                    CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                    resptBackS4Vec[k-9].push_back(ptsOut);
                }
            }
        }

        #pragma omp section
        {
            //S5
            int i=25;
            for(auto k=11;k<13;k++)
            {
                std::vector<int> tparas;
                for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
                {
                    tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(k,j));
                }
                //paras.push_back(tparas);
                std::vector<cv::Rect> rangVec;

                if(srcImgBack.empty()) {
                    Q_ASSERT(0);
                }

                TemplateMatch(srcImgBack,m_dataAllVec[i],rangVec);
                cv::Mat imgt=srcImgBack(rangVec[0]).clone();
                std::vector<cv::Point> contours;
                MeasurE452EdgeDetect obj;
                obj.GetLineContours(imgt,tparas,contours);
                ImageCoordinateTransformation(contours,rangVec);

                std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
                while (!MeasurE452AlgBase::flagVec[id])
                {
                    (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
                }
                for(auto &elem:contours)
                {
                    cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
                    CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                    resptBackS5Vec[k-11].push_back(ptsOut);
                }
            }
        }

        #pragma omp section
        {
            //S3high
            int i=40;
            for(auto k=5;k<6;k++)
            {
                std::vector<int> tparas;
                for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
                {
                    tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(k,j));
                }
                //paras.push_back(tparas);
                std::vector<cv::Rect> rangVec;


                TemplateMatch(srcImgHigh,m_dataAllVec[i],rangVec);
                cv::Mat imgt=srcImgHigh(rangVec[0]).clone();
                std::vector<cv::Point> contours;
                MeasurE452EdgeDetect obj;
                obj.GetLineContours(imgt,tparas,contours);
                ImageCoordinateTransformation(contours,rangVec);

                std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
                while (!MeasurE452AlgBase::flagVec[id])
                {
                    (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
                }
                for(auto &elem:contours)
                {
                    cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
                    CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                    resptHighS3Vec[k-5].push_back(ptsOut);
                }
            }
        }

    }

    std::vector<cv::Point> resptVec;

    for(auto i=8;i<12;i++)
    {
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-8,j));
        }
        //paras.push_back(tparas);
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImgBack,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImgBack(rangVec[0]).clone();
        std::vector<cv::Point> contours;
        MeasurE452EdgeDetect obj;
        obj.NewCircleEdge(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*MeasurE452AlgBase::mtxptrVec[id]);
        while (!MeasurE452AlgBase::flagVec[id])
        {
            (*MeasurE452AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            resptVec.push_back(ptsOut);
        }
    }

    // go on coding
    //cout<<resptVec[3].x<<"\t"<<resptVec[3].y<<endl;
    MeasurE452EdgeDetect obj;
    vector<float> cirres=obj.FitCircle(resptVec);

    // go on coding
    //cout<<resptBackS4Vec[0][3].x<<"\t"<<resptBackS4Vec[0][3].y<<endl;

    float fai29=obj.GetLineDist(resptBackS1Vec[1],resptBackS1Vec[4],1);
    measRes.insert(std::make_pair(string("FAI29-S1-X-distance_pixel"), fai29));
    float fai29s1x = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI29-S1-X-distance")], fai29);
    measRes.insert(std::make_pair(string("FAI29-S1-X-distance_real"), fai29s1x));

    vector<cv::Point2f> botline=resptBackS1Vec[2];
    botline.insert(botline.end(),resptBackS1Vec[3].begin(),resptBackS1Vec[3].end());
    float fai30=obj.GetLineDist(botline,resptBackS1Vec[0],2);
    measRes.insert(std::make_pair(string("FAI30-S1-Y-distance_pixel"), fai30));
    float fai30s1y = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI30-S1-Y-distance")], fai30);
    measRes.insert(std::make_pair(string("FAI30-S1-Y-distance_real"), fai30s1y));

    float sumx31p9=0;
    for(int i=0;i<resptBackS4Vec[0].size();++i){
        sumx31p9+=resptBackS4Vec[0][i].y;
    }
    float fai31p9x=fabs(sumx31p9/resptBackS4Vec[0].size()-cirres[1]);
    measRes.insert(std::make_pair(string("FAI31-S4P9-X-distance_pixel"), fai31p9x));
    float fai31p9xr = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI31-S4P9-X-distance")], fai31p9x);
    measRes.insert(std::make_pair(string("FAI31-S4P9-X-distance_real"), fai31p9xr));

    float sumx31p10=0;
    for(int i=0;i<resptBackS4Vec[1].size();++i){
        sumx31p10+=resptBackS4Vec[1][i].y;
    }
    float fai31p10x=fabs(sumx31p10/resptBackS4Vec[1].size()-cirres[1]);
    measRes.insert(std::make_pair(string("FAI160-S4P9-X-distance_pixel"), fai31p10x));
    float fai31p10xr = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI160-S4P9-X-distance")], fai31p10x);
    measRes.insert(std::make_pair(string("FAI160-S4P9-X-distance_real"), fai31p10xr));

    float sumx160p9=0;
    for(int i=0;i<resptBackS5Vec[0].size();++i){
        sumx160p9+=resptBackS5Vec[0][i].y;
    }
    float fai160p9x=fabs(sumx160p9/resptBackS5Vec[0].size()-cirres[1]);
    measRes.insert(std::make_pair(string("FAI31-S5P10-X-distance_pixel"), fai160p9x));
    float fai160p9xr = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI31-S5P10-X-distance")], fai160p9x);
    measRes.insert(std::make_pair(string("FAI31-S5P10-X-distance_real"), fai160p9xr));

    float sumx160p10=0;
    for(int i=0;i<resptBackS5Vec[1].size();++i){
        sumx160p10+=resptBackS5Vec[1][i].y;
    }
    float fai160p10x=fabs(sumx160p10/resptBackS5Vec[1].size()-cirres[1]);
    measRes.insert(std::make_pair(string("FAI160-S5P10-X-distance_pixel"), fai160p10x));
    float fai160p10xr = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI160-S5P10-X-distance")], fai160p10x);
    measRes.insert(std::make_pair(string("FAI160-S5P10-X-distance_real"), fai160p10xr));

    float sumx52=0;
    for(int i=0;i<resptBackS2Vec[3].size();++i){
        sumx52+=resptBackS2Vec[3][i].x;
    }
    float fai52=sumx52/resptBackS2Vec[3].size();
    measRes.insert(std::make_pair(string("FAI52-S2-X-distance_pixel"), fai52));
    float fai52r = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI52-S2-X-distance")], fai52);
    measRes.insert(std::make_pair(string("FAI52-S2-X-distance_real"), fai52r));

    float sum53s21=0,sum53s22=0;
    for(int i=0;i<resptBackS2Vec[0].size();++i){
        sum53s21+=resptBackS2Vec[0][i].y;
    }
    for(int i=0;i<resptBackS2Vec[2].size();++i){
        sum53s22+=resptBackS2Vec[2][i].y;
    }
    float fai53=fabs(sum53s21/resptBackS2Vec[0].size()+sum53s22/resptBackS2Vec[2].size())/2.0;
    measRes.insert(std::make_pair(string("FAI53-S2-Y-distance_pixel"), fai53));
    float fai53r = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI53-S2-Y-distance")], fai53);
    measRes.insert(std::make_pair(string("FAI53-S2-Y-distance_real"), fai53r));

    float fai58=obj.GetLineDist(resptBackS2Vec[2],resptBackS2Vec[0],2);
    measRes.insert(std::make_pair(string("FAI58-S2-Y-distance_pixel"), fai58));
    float fai58r = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI58-S2-Y-distance")], fai58);
    measRes.insert(std::make_pair(string("FAI58-S2-Y-distance_real"), fai58r));

    float sum59s21=0,sum59s22=0;
    for(int i=0;i<resptBackS2Vec[3].size();++i){
        sum59s21+=resptBackS2Vec[3][i].x;
    }
    for(int i=0;i<resptHighS3Vec[0].size();++i){
        sum59s22+=resptHighS3Vec[0][i].x;
    }
    float fai59=fabs(sum59s21/resptBackS2Vec[3].size()-sum59s22/resptHighS3Vec[0].size());;
    measRes.insert(std::make_pair(string("FAI59-S2S3-X-distance_pixel"), fai59));
    float fai59r = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI59-S2S3-X-distance")], fai59);
    measRes.insert(std::make_pair(string("FAI59-S2S3-X-distance_real"), fai59r));

    float fai60=obj.GetLineDist(resptBackS2Vec[1],resptBackS2Vec[3],1);
    measRes.insert(std::make_pair(string("FAI60-S2-X-distance_pixel"), fai60));
    float fai60r = CalcMeasureDistance(m_phypixpara[fixtureId][std::string("FAI60-S2-X-distance")], fai60);
    measRes.insert(std::make_pair(string("FAI60-S2-X-distance_real"), fai60r));

    qDebug() << "end to E452 alg back s1 s2 s4 s5 high s3 circle";
}
